#include <mc_common/path_utils.h>

namespace path_utils{

// 获取TF坐标变换
bool GetTFPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame, ros::Time stamp){
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = child_frame;
    robot_pose.header.stamp = ros::Time();
    // get the global pose of the robot
    try
    {
        // use current time if possible (makes sure it's not in the future)
        if (tf_buffer->canTransform(father_frame, child_frame, stamp))
        {
            geometry_msgs::TransformStamped transform = tf_buffer->lookupTransform(father_frame, child_frame, stamp);
            tf2::doTransform(robot_pose, global_pose, transform);
        }
        // use the latest otherwise
        else
        {
            tf_buffer->transform(robot_pose, global_pose, father_frame);
        }
    }
    catch (tf2::LookupException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    // // check global_pose timeout
    // ros::Time current_time = ros::Time::now();
    // if(current_time.toSec() - global_pose.header.stamp.toSec() > __transform_tolerance)
    // {
    //     ROS_WARN_THROTTLE(1.0,
    //                     "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
    //                     current_time.toSec(), global_pose.header.stamp.toSec(), __transform_tolerance);
    //     return false;
    // }

    return true;        
}

}; //naespace path_utils